#Copyright (c) 2020 Ocado. All Rights Reserved.

import time, math, random
import numpy as np

from amrrt.tree import Tree
from amrrt.metrics import EuclideanMetric
from amrrt.cost import Cost


class RTSamplingPlanner:
    """
    Real time sampling planner, parent class for RTRRT and AMRRT
    实时采样规划器，RTRRT 和 AMRRT 的父类
    """
    def __init__(self, space, agent_pos, assisting_metric=None, t_exp=0.15, t_root=0.003, t_steer=0.002):
        """
        :param space: State space planner operates in
        :param agent_pos: Initial position of agent
        :param assisting_metric: Assisting metric for the planner, default None for Euclidean
        :param t_exp: Time spent expanding and rewiring
        :param t_root: Time spent rewiring at the root
        :param t_steer: Time spent steering


         :param space: 状态空间规划器在
         :param agent_pos: 代理的初始位置
         :param assisting_metric: 规划器的辅助指标，欧几里德默认无
         :param t_exp: 扩展和重新布线所花费的时间
         :param t_root: 在根目录重新布线所花费的时间
         :param t_steer: 转向时间
        """
        self.space = space
        self.euclidean_metric = EuclideanMetric()#使用一个对象
        self.assisting_metric = assisting_metric if assisting_metric is not None else self.euclidean_metric
        self.euclidean_distance = lambda a, b : self.euclidean_metric.distance(a, b)
        self.assisting_distance = lambda a, b : self.assisting_metric.distance(a, b)
        #算距离的
        #得当前对象可以调用这个lambda函数来计算两个输入对象之间的距离，而不需要直接访问assisting_metric对象和它的distance方法。
        self.tree = Tree(self.space.create_state(agent_pos), self.space, assisting_dist_fn=self.assisting_distance)
        self.goal = None
        self.root_queue = []
        self.rewired_root = set()
        self.t_exp, self.t_root, self.t_steer = t_exp, t_root, t_steer

    def _nearest_node(self, node):
        """

        Find nearest node, first by Euclidean metric if the path is free or fallback on the assisting metric
        先通过欧几里得度量找到最近的节点，如果路径是可行的，则直接使用欧几里得度量；否则，回退到辅助度量来寻找最近的节点。
        """
        euclidean = self.tree.euclidean_nearest(node)
        #在tree的euclidean_vpt里找最近的点
        if self.space.free_path(node, euclidean) or self.euclidean_metric == self.assisting_metric:
            return euclidean
        return self.tree.nearest(node)
         # 在tree的assisting_vpt里找最近的点

    def set_root(self, root):
        """
        Set the root node for the planner tree, and reset rewiring queue
        为规划树设置根节点，并重置重布线队列
        """
        self.tree.set_root(root)
        self.root_queue = [self.tree.root]
        self.rewired_root = {self.tree.root}

    def set_goal(self, pos):
        """
        Set new goal from position, try to add goal to tree if within range & path free
        从位置设置新目标，如果在范围和路径自由范围内，尝试将目标添加到树
        """
        self.goal = self.space.create_state(pos)
        xnearest = self._nearest_node(self.goal)
        #先通过欧几里得度量找到最近的节点，如果路径是可行的，则直接使用欧几里得度量；否则，回退到辅助度量来寻找最近的节点。
        if self.euclidean_distance(xnearest, self.goal) < self.max_step and self.space.free_path(xnearest, self.goal):
            #看一下能不能直接走到
            self.tree.add_node(self.goal, xnearest)

    def _add_node(self, xnew, xnearest, nearby):
        """
        Add new node to the tree, connecting it to the node in nearby that minimises cost
        Also attempt to add the goal to the tree via xnew if within range & free path
        将一个新节点添加到树中，连接到在附近的节点中使代价最小的节点。
        如果目标状态在范围内且路径上没有障碍，则也尝试通过新的节点将目标状态添加到树中。
        函数返回被添加到树中的新节点。

        :param xnew: New state to be added
        :param xnearest: Nearest node to xnew
        :param nearby: Set of nodes in the neighbourhood of xnew

        xnew：要添加的新状态
        xnea    rest：与新状态最近的节点
        near    by：在新状态附近的节点集合

        """
        xmin = xnearest
        cmin = self.cost(xnearest) + self.euclidean_distance(xnearest, xnew)
        for xnear in nearby:
            cnew = self.cost(xnear) + self.euclidean_distance(xnear, xnew)
            if cnew < cmin and self.space.free_path(xnear, xnew):
                xmin = xnear
                cmin = cnew
        self.tree.add_node(xnew, xmin)
        if self.goal is not None and self.euclidean_distance(xnew, self.goal) < self.max_step and self.space.free_path(xnew, self.goal):
            # 有一定概率终点
            self.tree.add_node(self.goal, xnew)

    def goal_path(self):
        """
        Return path to the goal if one exists, if not a path to the nearest node
        返回到目标的路径，如果不存在，则返回到最近节点的路径。
        """
        if self.goal is None:
            return []
        if self.goal in self.tree.nodes:
            return self.tree.path(self.goal)
        return self.tree.path(self._nearest_node(self.goal))

    def cost(self, node):
        """
        Returns path cost to given node (as Cost object)
        """
        if node == self.tree.root:
            return Cost(0, False)
        path = self.tree.path(node)
        running_cost = 0
        blocked = False
        for i in range(1, len(path)):
            if not self.space.dynamically_free_path(path[i-1], path[i]) : blocked = True
            running_cost += self.euclidean_distance(path[i-1], path[i])
        return Cost(running_cost, blocked)

    def add_dynamic_obstacle(self, pos, radius,Obs_num):
        """
        Add circular dynamic obstacle, and reset goal queues

        :param pos: Position of dynamic obstacle
        :param radius: Radius of dynamic obstacle

        添加一个圆形动态障碍物，并重置目标队列。

        :param pos: 动态障碍物的位置
        :param radius: 动态障碍物的半径
        """
        self.space.add_dynamic_obstacle(pos, radius,Obs_num)
        if self.goal is not None : self.set_goal(self.goal.pos)

class RTRRTPlanner(RTSamplingPlanner):
    def __init__(self, space, agent_pos, assisting_metric=None, t_exp=0.15, t_root=0.1, t_rand=0.03, t_steer=0.002):
        """
        t_exp=0.15, t_root=0.03, t_rand=0.03, t_steer=0.002
        :param space: State space planner operates in
        规划器所操作的状态空间
        :param agent_pos: Initial position of agent
        代理的初始位置
        :param assisting_metric: Assisting metric for the planner, default None for Euclidean
        规划器的辅助度量标准，默认为欧几里得距离
        :param t_exp: Time spent expanding and rewiring
        扩展和重构的时间
        :param t_root: Time spent rewiring at the root
        在根节点进行重构的时间
        :param t_rand: Time spent rewiring random nodes
        在随机节点进行重构的时间
        :param t_steer: Time spent steering
        转向所花费的时间
        """
        super().__init__(space, agent_pos, assisting_metric=assisting_metric, t_exp=t_exp, t_root=t_root, t_steer=t_steer)
        #super().__init__()是一个用于调用父类构造函数的方法。
        #当一个类继承自另一个类时，子类可以通过调用父类的构造函数来初始化父类的属性，同时在子类的构造函数中执行其他初始化操作。
        #此外，子类的构造函数也可以在调用 super().__init__() 之后执行其他的初始化操作。
        self.rand_queue = []
        self.t_rand = t_rand
        self.max_step = 5 # 5
        self.node_density = 12

    def plan(self, agent_pos):
        """
        Run main body of RT-RRT algorithm, explore & rewire to plan a path

        :param agent_pos: Agent's current position

        运行 RT-RRT 算法主体，探索并重新布线以规划路径

         :param agent_pos: 代理当前位置
        """
        agent = self.space.create_state(agent_pos)
        start = time.time()
        while time.time() - start < self.t_exp:
            self.expand()
        goal_path = self.goal_path()
        if self.euclidean_distance(agent, self.tree.root) < self.max_step/10 and len(goal_path) > 1:
            self.set_root(goal_path[1]) # 向着路径的第一个点前进
        return self.tree.root

    def expand(self):
        """
        这部分和RTRRT论文的伪代码一模一样
        Expand the tree through sampling & rewiring
        """
        xrand = self.sample_state()

        # 随机采点
        xnearest = self.tree.nearest(xrand)
        # 返回assisting_vpt中离 xrand 最近的点
        # 这里的tree是self实体的tree，也就是class RTRRTPlanner(RTSamplingPlanner):
        # 其中 self.tree 是由外部导入的 Tree类构造的，所以要用Tree类的.nearest方法
        xnew = self.steer(xnearest, xrand)
        #print("xnew",xnew.pos)
        #print(xnew[0],xnew[1])
        # xrand 有步长和障碍物限制
        if xnew is not None:
            nearby = self.find_nodes_near(xnew)
            # 找的是rrt树上的
            if len(nearby) < self.node_density or self.euclidean_distance(xnearest, xrand) > self.max_step or xnew == self.goal:
                self._add_node(xnew, xnearest, nearby)
                # 新加入点重布线
                self.rand_queue.insert(0, xnew)
                # 随机重布线
            else:
                self.rand_queue.insert(0, xnearest)
            self.rewire_rand()
            # 随机重布线
        self.rewire_root()
        # 起点重布线

    def find_nodes_near(self, node):
        radius = max(math.sqrt((self.space.area()*self.node_density) / (math.pi*self.tree.node_amount)), self.max_step)
        '''
        具体来说，代码中的计算过程如下：

        首先，计算空间中所有点的密度，即空间面积（area）乘以点的数量密度（node_density）的比例。

        然后，计算圆的面积，其中圆的半径是通过将上述密度值除以圆周率π得到的。这个圆代表了一个搜索半径，它包含了密度最高的点集。

        接着，比较上述搜索半径和一个预设的最大步长（max_step），取其中较大的一个值作为最终的搜索半径。这个步骤是为了确保搜索的半径不会超过预设的最大值，以避免搜索时间过长。

        最后，将计算得到的最大搜索半径 radius 返回。

        总的来说，这段代码的作用是根据点的分布密度和最大步长计算出一个合适的搜索半径，以便在树结构中搜索最近邻点时可以快速定位到周围的点。
        '''
        return self.tree.neighbourhood(node, radius)

    def rewire_root(self):
        """
        Rewire unseen nodes out from the root, resetting rewired_root once all nodes have been visited
        从根中重新连接未看到的节点，在访问完所有节点后重置rewired_root
        """
        if len(self.root_queue) == 0:
            self.root_queue.append(self.tree.root)
            self.rewired_root = {self.tree.root}
        start = time.time()
        while len(self.root_queue) > 0 and time.time() - start < self.t_root:
            xqueue = self.root_queue.pop()
            nearby = self.find_nodes_near(xqueue)
            for xnear in nearby:
                if self.cost(xnear) + self.euclidean_distance(xnear, xqueue) < self.cost(xqueue) and self.space.free_path(xnear, xqueue):
                    self.tree.update_edge(xnear, xqueue)
                    # 根节点重布线
                if xnear not in self.rewired_root:
                    # 重布线要经过rewired_root才可以（相当于两次，经过xnear后一定要到root_queue再重布线）
                    # rewired_root用来记录的
                    self.root_queue.insert(0, xnear)
                    self.rewired_root.add(xnear)
                    # print("根重布线队列大小", len(self.root_queue))


    def rewire_rand(self):
        """
        Rewire random portions of the graph via the rand_queue
        通过rand_queue随机重连部分图形。
        """
        start = time.time()
        while len(self.rand_queue) > 0 and time.time() - start < self.t_rand:
            xqueue = self.rand_queue.pop()
            nearby = self.find_nodes_near(xqueue)
            for xnear in nearby:
                if self.cost(xqueue) + self.euclidean_distance(xqueue, xnear) < self.cost(xnear) and self.space.free_path(xqueue, xnear):
                    # 和xqueue混了
                    self.tree.update_edge(xqueue, xnear)
                    self.rand_queue.insert(0, xnear)
                    # print("随机重布线队列大小", len(self.rand_queue))


    def sample_state(self, a=0.3, b=1.5):
        """
        Return a randomly sampled state from the state space

        :param a: Scales probability of sampling on the line between the goal and its current nearest neighbour
        :param b: Scales probability of sampling between entire state space and within rewire ellipse

        参数 a：调整采样概率，使其更有可能在目标点和其当前最近邻之间的连线上进行采样。
        参数 b：调整采样概率，使其更有可能在整个状态空间内以及重新调整椭圆内进行采样。
        """
        p = np.random.rand()
        if p > 1-a and self.goal is not None:
            r = np.random.rand()
            return self.space.create_state(r * self.goal.pos + (1-r) * self.tree.nearest(self.goal).pos)
        #则以概率 $1/(1-a)$ 在目标状态 goal 和其当前最近邻之间的连线上进行采样；
        elif p < (1-a)/b or self.goal is None or self.goal not in self.tree.nodes:
            return self.space.choose_state_uniform()
        else:
            cbest = self.cost(self.goal).to_float()
            cmin = self.euclidean_distance(self.tree.root, self.goal)
            return self.space.choose_state_ellipse(self.tree.root.pos, self.goal.pos, cbest, max(0, cbest**2 - cmin**2)**0.5)

    def steer(self, start, end):
        """
        Return a state that grows the tree towards end from start
        返回一个状态，使得从起点向终点生长树形结构。
        """
        return self.euclidean_metric.steer(self.space, start, end, np.inf, self.t_steer)
    # self.euclidean_metric = EuclideanMetric()#使用一个对象
    # .steer是EuclideanMetric()类方法，最前面也用from导入了

class SYRTRRTPlanner(RTRRTPlanner):

    def __init__(self, space, agent_pos, Approx,scale,assisting_metric=None,t_exp=0.2, t_root=0.08, t_rand=0.03,t_steer=0.002):
        super().__init__(space, agent_pos, assisting_metric=assisting_metric, t_exp=t_exp, t_root=t_root,
                         t_steer=t_steer)
        self.Approx=Approx
        self.startconvex=None
        self.visited_line=set()
        self.scale=scale
        self.obstacles_area=set()

    def sample_state(self, a=0.2, b=0.1,c=0.25):
        """
        Return a randomly sampled state from the state space

        :param a: Scales probability of sampling on the line between the goal and its current nearest neighbour
        :param b: Scales probability of sampling between entire state space and within rewire ellipse

        参数 a：调整采样概率，使其更有可能在目标点和其当前最近邻之间的连线上进行采样。
        参数 b：调整采样概率，使其更有可能在整个状态空间内以及重新调整椭圆内进行采样。
        """
        p = np.random.rand()
        p2= np.random.rand()
        #r = np.random.rand()
        r=0.5
        if p > 1-a and self.goal is not None and len(self.visited_line)!=0:
            return self.space.create_state(r * self.goal.pos + (1-r) * self.tree.nearest(self.goal).pos)
        #则以概率 $1/(1-a)$ 在目标状态 goal 和其当前最近邻之间的连线上进行采样；
        elif len(self.visited_line)==0:
            choice_line=self.Approx.islandlist[self.startconvex].cutline[0]#切割线编号
            self.visited_line.add(choice_line)
            line_start=np.divide(self.Approx.point_map[self.Approx.cutlinelist[choice_line][0]],self.scale)#切割线起始点编号，查一下
            line_end = np.divide(self.Approx.point_map[self.Approx.cutlinelist[choice_line][1]],self.scale)
            line_start = np.array(line_start, dtype="float")
            line_end = np.array(line_end, dtype="float")
            return self.space.create_state(r * line_start + (1 - r) * line_end)
        elif len(self.visited_line) < len(self.Approx.cutlinelist) and len(self.visited_line)!=0:
            #random_nodes = random.sample(self.tree.nodes, 1)[0]
            visit_flag=True
            while visit_flag:
                random_line = random.sample(self.visited_line, 1)[0] # 选一个已经采样过的切割线
                choice_line = random.choice(self.Approx.bridgelist[random_line].bridge2bridge) # 切割线（桥）能去哪些桥
                if choice_line not in self.visited_line:
                    visit_flag=False
                    self.visited_line.add(choice_line)
                    line_start=np.divide(self.Approx.point_map[self.Approx.cutlinelist[choice_line][0]],self.scale)
                    #切割线起始点编号，查一下
                    #要转化成space空间
                    line_end =np.divide( self.Approx.point_map[self.Approx.cutlinelist[choice_line][1]],self.scale)
                    line_start = np.array(line_start, dtype="float")
                    line_end = np.array(line_end, dtype="float")
                    return self.space.create_state(r * line_start + (1 - r) * line_end)

        # elif p < (1-a)/b or self.goal is None or self.goal not in self.tree.nodes:
        #     return self.space.choose_state_uniform()
        elif self.goal is not None and len(self.visited_line) >= len(self.Approx.cutlinelist) and self.goal in self.tree.nodes and p2>b and p2 <c:
            print('goal')
            cbest = self.cost(self.goal).to_float()
            cmin = self.euclidean_distance(self.tree.root, self.goal)
            return self.space.choose_state_ellipse(self.tree.root.pos, self.goal.pos, cbest, max(0, cbest**2 - cmin**2)**0.5)
        elif len(self.obstacles_area)>0 and len(self.visited_line) >= len(self.Approx.cutlinelist) and p2>c:
            find=False
            while find!=True:
                arr_list =list(self.obstacles_area)
                arr = np.array(arr_list)
                weights = np.arange(1, len(arr) + 1)  # 设置权重，这里采用下标来表示
                random_area = np.random.choice(arr, p=weights / weights.sum())  # 根据权重进行随机抽取

                #random_area = random.sample(self.obstacles_area, 1)[0]
                # print(random_area)
                if random_area>=len(self.Approx.islandlist):
                    continue
                boundary=self.Approx.islandlist[random_area].boundary
                random_x = random.randint(boundary[0], boundary[1])
                random_y = random.randint(boundary[2], boundary[3])
                random_point=[int(random_x), int(random_y)]
                if self.Approx.IMGmap[int(random_y), int(random_x)][0] in self.obstacles_area:
                    random_point_inspace = np.divide(random_point, self.scale)
                    if self.space.free_position(random_point_inspace):
                        # print(random_point)
                        # print(random_point_inspace)
                        return self.space.create_state(random_point_inspace)
        else :
            #r = np.random.rand()
            r = np.random.rand()
            random_line = random.sample(self.visited_line, 1)[0]  # 选一个已经采样过的切割线
            choice_line = random.choice(self.Approx.bridgelist[random_line].bridge2bridge)  # 切割线（桥）能去哪些桥
            line_start = np.divide(self.Approx.point_map[self.Approx.cutlinelist[choice_line][0]],self.scale)  # 切割线起始点编号，查一下
            line_end = np.divide(self.Approx.point_map[self.Approx.cutlinelist[choice_line][1]],self.scale)
            line_start = np.array(line_start, dtype="float")
            line_end = np.array(line_end, dtype="float")
            return self.space.create_state(r * line_start + (1 - r) * line_end)

    def expand(self):
        """
        这部分和RTRRT论文的伪代码一模一样
        Expand the tree through sampling & rewiring
        """
        xrand= self.sample_state()

        # 随机采点
        xnearest = self.tree.nearest(xrand)

        if self.space.free_path(xrand, xnearest)==False and len(self.visited_line) < len(self.Approx.cutlinelist):#之前应为有steer函数，可以不考虑障碍物采点，现在要考虑上
            cmin=float('inf')
            xmin=xnearest
            for node in self.tree.nodes:
                cnew=self.euclidean_distance(xrand, node)
                if cnew < cmin and self.space.free_path(node, xrand):
                    xmin = node
                    cmin = cnew
            xnearest=xmin
        elif len(self.visited_line) >= len(self.Approx.cutlinelist):
            xnew = self.steer(xnearest, xrand)
            #print('xnew = self.steer(xnearest, xrand)')


        # 返回assisting_vpt中离 xrand 最近的点
        # 这里的tree是self实体的tree，也就是class RTRRTPlanner(RTSamplingPlanner):
        # 其中 self.tree 是由外部导入的 Tree类构造的，所以要用Tree类的.nearest方法
        # xnew = self.steer(xnearest, xrand) #self.steer会躲障碍物，可能会采不到切割线，刚开始一定要强制采在切割线上
        if len(self.visited_line) < len(self.Approx.cutlinelist):
            #print('xnew=xrand')
            xnew=xrand
        #print("xnew1",xnew.pos)
        # xrand 有步长和障碍物限制
        if xnew is not None:
            nearby = self.find_nodes_near(xnew)
            # 找的是rrt树上的
            if len(nearby) < self.node_density or self.euclidean_distance(xnearest, xrand) > self.max_step or xnew == self.goal:
                self._add_node(xnew, xnearest, nearby)
                # 新加入点重布线
                self.rand_queue.insert(0, xnew)
                # 随机重布线
            else:
                self.rand_queue.insert(0, xnearest)
            self.rewire_rand()
            # 随机重布线
        self.rewire_root()
        # 起点重布线

    def _add_node(self, xnew, xnearest, nearby):
        """
        Add new node to the tree, connecting it to the node in nearby that minimises cost
        Also attempt to add the goal to the tree via xnew if within range & free path
        将一个新节点添加到树中，连接到在附近的节点中使代价最小的节点。
        如果目标状态在范围内且路径上没有障碍，则也尝试通过新的节点将目标状态添加到树中。
        函数返回被添加到树中的新节点。

        :param xnew: New state to be added
        :param xnearest: Nearest node to xnew
        :param nearby: Set of nodes in the neighbourhood of xnew

        xnew：要添加的新状态
        xnea    rest：与新状态最近的节点
        near    by：在新状态附近的节点集合

        """
        xmin = xnearest

        if self.space.free_path(xnew, xnearest):
            cmin = self.cost(xnearest) + self.euclidean_distance(xnearest, xnew)
        else:
            cmin = float('inf')

        for xnear in nearby:
            cnew = self.cost(xnear) + self.euclidean_distance(xnear, xnew)
            if cnew < cmin and self.space.free_path(xnear, xnew):
                xmin = xnear
                cmin = cnew
        if cmin==float('inf'):
            print("11111")
        self.tree.add_node(xnew, xmin)
        if self.goal is not None and self.euclidean_distance(xnew, self.goal) < self.max_step and self.space.free_path(xnew, self.goal):
            # 有一定概率终点
            self.tree.add_node(self.goal, xnew)


class AMRRTPlanner(RTSamplingPlanner):
    def __init__(self, space, agent_pos, assisting_metric=None, t_exp=0.15, t_root=0.002, t_goal=0.004, t_steer=0.002):
        """
        :param space: State space planner operates in
        :param agent_pos: Initial position of agent
        :param assisting_metric: Assisting metric for the planner, default None for Euclidean
        :param t_exp: Time spent expanding and rewiring
        :param t_root: Time spent rewiring at the root
        :param t_goal: Time spent rewiring at the goal
        :param t_steer: Time spent steering
        """
        super().__init__(space, agent_pos, assisting_metric=assisting_metric, t_exp=t_exp, t_root=t_root, t_steer=t_steer)
        self.goal_stack = []
        self.goal_queue = []
        self.t_goal = t_goal
        self.max_step = (self.space.bounds[0,1] - self.space.bounds[0,0]) * 0.05
        self.node_density = 20

    def plan(self, agent_pos):
        """
        Run main body of AM-RRT algorithm, explore & rewire to plan a path

        :param agent_pos: Agent's current position

        运行 AM-RRT 算法主体，探索并重新布线以规划路径

         :param agent_pos: 代理当前位置
        """
        #print("1234")
        agent = self.space.create_state(agent_pos)
        start = time.time()
        while time.time() - start < self.t_exp:
            self.expand()
        goal_path = self.goal_path()
        if self.euclidean_distance(agent, self.tree.root) < self.max_step/10 and len(goal_path) > 1:
            self.set_root(goal_path[1])
        return self.tree.root

    def expand(self):
        """
        Expand the tree through sampling & rewiring
        """
        xrand = self.sample_state()
        xnearest = self._nearest_node(xrand)
        xnew = self.steer(xnearest, xrand)
        if xnew is not None and xnew not in self.tree.nodes:
            nearby = self.tree.neighbourhood(xnew, self.max_step)
            if len(nearby) < self.node_density or self.euclidean_distance(xnearest, xrand) > self.max_step or xnew == self.goal:
                self._add_node(xnew, xnearest, nearby)
        self.rewire_root()
        if self.goal is not None and self.goal in self.tree.nodes:
            self.rewire_goal()

    def sample_state(self, a=0.3, b=1.5):
        """
        Return a randomly sampled state from the state space

        :param a: Scales probability of sampling on the line between the goal and its current nearest neighbour
        :param b: Scales probability of sampling between entire state space and within rewire ellipse
        """
        p = np.random.rand()
        if p > 1-a and self.goal is not None and self.goal not in self.tree.nodes:
            return self.space.create_state(self.goal.pos)
        elif p < (1-a)/b or self.goal is None or self.goal not in self.tree.nodes:
            return self.space.choose_state_uniform()
        else:
            cbest = self.cost(self.goal).to_float()
            cmin = self.euclidean_distance(self.tree.root, self.goal)
            return self.space.choose_state_ellipse(self.tree.root.pos, self.goal.pos, cbest, max(0, cbest**2 - cmin**2)**0.5)

    def rewire_root(self):
        """
        Rewire unseen nodes out from the root, resetting rewired_root once all nodes have been visited、
        从根中重新连接看不见的节点，一旦所有节点都被访问过，就重置 rewired_root
        """
        if len(self.root_queue) == 0:
            self.root_queue.append(self.tree.root)
            self.rewired_root = {self.tree.root}
        start = time.time()
        while len(self.root_queue) > 0 and time.time() - start < self.t_root:
            xrewire = self.root_queue.pop()
            nearby = self.tree.neighbourhood(xrewire, self.max_step)
            for xnear in nearby:
                if self.cost(xnear) + self.euclidean_distance(xnear, xrewire) < self.cost(xrewire) and self.space.free_path(xnear, xrewire):
                    self.tree.update_edge(xnear, xrewire)
                if xnear not in self.rewired_root:
                    self.root_queue.insert(0, xnear)
                    self.rewired_root.add(xnear)

    def rewire_goal(self):
        """
        Rewire unseen nodes out from the root towards the goal along offshoots
        沿着分支将看不见的节点从根部重新连接到目标上
        """
        if len(self.goal_stack) == 0 and len(self.goal_queue) == 0:
            self.goal_stack.append(self.tree.root)
            self.seen_goal = set()
        start = time.time()
        while time.time() - start < self.t_goal and (len(self.goal_stack) > 0 or len(self.goal_queue) > 0):
            if len(self.goal_stack) > 0 : xrewire = self.goal_stack.pop()
            else : xrewire = self.goal_queue.pop()
            cbest = self.cost(self.goal).to_float()
            cmin = self.euclidean_distance(self.tree.root, self.goal)
            if self._within_ellipse(xrewire.pos, self.tree.root.pos, self.goal.pos, cbest/2, max(0, cbest**2-cmin**2)**0.5/2):
                nearby = self.tree.neighbourhood(xrewire, self.max_step)
                rev_sorted_indexes = sorted([(-self.assisting_distance(self.goal, nearby[i]), i) for i in range(len(nearby))])
                nearby_sorted = [nearby[i] for _, i in rev_sorted_indexes]
                frontier = []
                for xnear in nearby_sorted:
                    if self.cost(xrewire) + self.euclidean_distance(xrewire, xnear) < self.cost(xnear) and self.space.free_path(xrewire, xnear):
                        self.tree.update_edge(xrewire, xnear)
                    if xnear not in self.seen_goal and self.space.free_path(xrewire, xnear):
                        frontier.append(xnear)
                        self.seen_goal.add(xnear)
                self.goal_stack = self.goal_stack + frontier
                self.goal_queue = frontier + self.goal_queue
            if len(self.goal_stack) > 0 and self.assisting_distance(self.goal_stack[-1], self.goal) > self.assisting_distance(xrewire, self.goal):
                self.goal_stack = []

    def steer(self, start, end):
        """
        Return a state that grows the tree towards end from start
        """
        if self.space.free_path(start, end):
            p = min(1, self.max_step/self.euclidean_distance(start, end))
            return self.space.create_state((1-p)*start.pos + p*end.pos)
        return self.assisting_metric.steer(self.space, start, end, self.max_step, self.t_steer)

    def set_goal(self, pos):
        """
        Set new goal from position, try to add goal to tree if within range & path free, and rest goal stack/queue
        """
        super().set_goal(pos)
        self.goal_stack = [self.tree.root]
        self.goal_queue = []
        self.seen_goal = {self.tree.root}

    def _within_ellipse(self, pos, fa, fb, major, minor):
        """
        Check if point falls within given ellipse

        :param pos: Position of point being checked
        :param fa: Position of first ellipse focus
        :param fb: Position of second ellipse focus
        :param major: Length of major axis
        :param minor: Length of minor axis
        """
        c = (fa + fb) / 2
        r = np.arctan2((fa-fb)[1], (fa-fb)[0])
        return (((np.cos(r)*(pos[0]-c[0]) + np.sin(r)*(pos[1]-c[1]))/major)**2 +
                ((np.sin(r)*(pos[0]-c[0]) - np.cos(r)*(pos[1]-c[1]))/minor)**2 <= 1)
